#include "ros/ros.h"
#include "sensor_msgs/Range.h"
#include "geometry_msgs/Twist.h"

const float OBSTACLE_DISTANCE = 0.5;

ros::Publisher cmd_vel_pub;
bool obstacle_detected = false;

void leftCallback(const sensor_msgs::Range::ConstPtr &msg) {
    if (msg->range < OBSTACLE_DISTANCE) {
        ROS_INFO("Obstacle detected on the left side.");
        obstacle_detected = true;
    }
}

void centerCallback(const sensor_msgs::Range::ConstPtr &msg) {
    if (msg->range < OBSTACLE_DISTANCE) {
        ROS_INFO("Obstacle detected in the center.");
        obstacle_detected = true;
    }
}

void rightCallback(const sensor_msgs::Range::ConstPtr &msg) {
    if (msg->range < OBSTACLE_DISTANCE) {
        ROS_INFO("Obstacle detected on the right side.");
        obstacle_detected = true;
    }
}

void rotateLeft() {
    ROS_INFO("Rotate left.");
    geometry_msgs::Twist twist;
    twist.angular.z = 0.5;
    cmd_vel_pub.publish(twist);
}

void rotateRight() {
    ROS_INFO("Rotate right.");
    geometry_msgs::Twist twist;
    twist.angular.z = -0.5;
    cmd_vel_pub.publish(twist);
}

void stopRobot() {
    ROS_INFO("Stop the robot.");
    geometry_msgs::Twist twist;
    twist.linear.x = 0;
    twist.angular.z = 0;
    cmd_vel_pub.publish(twist);
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "cre_obstacle_avoidance");
    ros::NodeHandle nh;

    ros::Subscriber left_sub = nh.subscribe<sensor_msgs::Range>("left_distance", 1000, leftCallback);
    ros::Subscriber center_sub = nh.subscribe<sensor_msgs::Range>("center_distance", 1000, centerCallback);
    ros::Subscriber right_sub = nh.subscribe<sensor_msgs::Range>("right_distance", 1000, rightCallback);

    cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1000);

    ros::Rate loop_rate(10);

    while (ros::ok()) {
        if (obstacle_detected) {
            obstacle_detected = false;
            rotateLeft();
            ros::Duration(2.0).sleep(); // 转向后暂停2秒，等待机器人转向if (obstacle_detected) // 左侧有障碍物，则向右转
            {
                obstacle_detected = false;
                rotateRight();
                ros::Duration(2.0).sleep();
                if (obstacle_detected) // 右侧也有障碍物，则停止机器人
                {
                    stopRobot();
                }
            }
        } else {
            ROS_INFO("No obstacle detected, move forward.");
            geometry_msgs::Twist twist;
            twist.linear.x = 0.5;
            cmd_vel_pub.publish(twist);
        }

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}